#include "SonarSensor.h"
#include "math.h"

SonarSensor::SonarSensor(uint32_t port, uint8_t moduleNumber = 1) :
	m_port(port),
	m_moduleNumber(moduleNumber)
{
	
}

SonarSensor::~SonarSensor()
{
	
}

bool SonarSensor::AtMaxDistance()
{
	if(AnalogModule::GetInstance(m_moduleNumber)->GetVoltage(m_port) > 4.95)
	{
		return true;
	}
	else
	{
		return false;
	}
}

double SonarSensor::GetDistanceInInches()
{
	double maxDistance = 254.0;
	double minDistance = 0;
	double maxVoltage = 5;
	double minVoltage = 0;
	
	return ((AnalogModule::GetInstance(m_moduleNumber)->GetVoltage(m_port) - minVoltage)/(maxVoltage - minVoltage) * (maxDistance - minDistance) + minDistance);
	
	
}
